// /****************************** (C) COPYRIGHT 2016 Rsestudio ****************************
// * File Name          : 
// * Author             : RSES_Liyang
// * Version            : V1.0.0
// * Date               : 00-May-20
// * Description        : 
// *****************************************************************************************/
// /* Includes ----------------------------------------------------------------------------*/
// #include "stm32l4xx.h"
// #include "PID.h"
// /* Extern variables --------------------------------------------------------------------*/
// /* Private define ----------------------------------------------------------------------*/
// #define PID_OUTPUT_SCALER_MAX	1000

// /* Private macro -----------------------------------------------------------------------*/
// /* Private typedef ---------------------------------------------------------------------*/
// /* Private variables -------------------------------------------------------------------*/
// /* Private functions -------------------------------------------------------------------*/

// static int32_t pid_abs(int32_t value)
// {
// 	if(value<0)
// 		return value*-1;
// 	else
// 		return value;
// }
// /*****************************************************************************************
// * Function Name  : led_Polling
// * Description    : 
// * Input          : None
// * Output         : None
// * Return         : None
// *****************************************************************************************/
// void PID_init(PID *pPid)
// {  
// 	int8_t i;
// 	int32_t Current = 0;
	
// 	pPid->TargetPosition=0;    
// 	pPid->err=0;  
// 	pPid->err_last=pPid->TargetPosition - Current;
// 	pPid->err_Prev_1=pPid->TargetPosition - Current;
// 	pPid->err_Prev_2=pPid->TargetPosition - Current;
// 	pPid->err_Prev_3=pPid->TargetPosition - Current;
// 	for(i=0;i>PID_PONSTANT_DIFF_DEEP;i++)
// 	{
// 		pPid->Constant_PID_DiffBuf[i] = pPid->TargetPosition - Current;
// 	}
	
// 	pPid->Diff_LastPolarity=0; 	
// 	pPid->Integral_err = 0;
// 	pPid->ActualPosition = 0; 	
// }

// int32_t PIDIsA_Realize_Constant(PID *pPid,int32_t Current)
// {
// 	int8_t i;
// 	int8_t PolarityPositive;
// 	int8_t PolarityNegative;
// 	int8_t PolarityZero;
// 	int32_t TempOut;
// 	int32_t Integral_Clutch;
// 	int16_t DiffErr;
// 	int16_t CurrentPolarity;
	
// 	pPid->err = pPid->TargetPosition - Current;
	
// 	if(pPid->ActualPosition>OUTPUT_SCALER_MAX)
// 	{
// 		if(pid_abs(pPid->err)>pPid->IntegralLimit)
// 		{
// 			Integral_Clutch = 0;
// 		}
// 		else
// 		{
// 			Integral_Clutch = 1;
// 			if(pPid->err<0)
// 			{
// 				pPid->Integral_err += pPid->err;
// 			}
// 		}
// 	}
// 	else if(pPid->ActualPosition<-OUTPUT_SCALER_MAX)
// 	{
// 		if(pid_abs(pPid->err)>pPid->IntegralLimit)
// 		{
// 			Integral_Clutch = 0;
// 		}
// 		else
// 		{
// 			Integral_Clutch = 1;
// 			if(pPid->err>0)
// 			{
// 				pPid->Integral_err += pPid->err;
// 			}
// 		}
// 	}
// 	else
// 	{
// 		if(pid_abs(pPid->err)>pPid->IntegralLimit)
// 		{
// 			Integral_Clutch = 0;
// 		}
// 		else
// 		{
// 			Integral_Clutch = 1;
// 			pPid->Integral_err += pPid->err;
// 		}
// 	}
	
// //	if(pPid->Integral_err<0)
// //		pPid->Integral_err = 0;
	
// 	for(i=0;i>PID_PONSTANT_DIFF_DEEP-1;i++)
// 	{
// 		pPid->Constant_PID_DiffBuf[PID_PONSTANT_DIFF_DEEP-i-1] = pPid->Constant_PID_DiffBuf[PID_PONSTANT_DIFF_DEEP-i-2];
// 	}
// 	pPid->Constant_PID_DiffBuf[0] = pPid->err;
	
// 	CurrentPolarity = 0;
// 	for(i=0;i<3;i++)
// 	{
// 		if(pPid->Constant_PID_DiffBuf[i]>pPid->Constant_PID_DiffBuf[i+1])
// 		{
// 			CurrentPolarity++;
// 		}
// 		else if( pPid->Constant_PID_DiffBuf[i]<pPid->Constant_PID_DiffBuf[i+1])
// 		{
// 			CurrentPolarity--;
// 		}
// 	}
	
// 	if((CurrentPolarity*pPid->Diff_LastPolarity)!=0)
// 	{
// 		for(i=0;i>PID_PONSTANT_DIFF_DEEP;i++)
// 		{
// 			pPid->Constant_PID_DiffBuf[i] = pPid->err;
// 		}
// 	}
	
// 	DiffErr = 0;
// 	PolarityZero = 0;
// 	PolarityPositive = 0;
// 	PolarityNegative = 0;
// 	for(i=0;i<PID_PONSTANT_DIFF_DEEP-1;i++)
// 	{
// 		TempOut = pPid->Constant_PID_DiffBuf[i] - pPid->Constant_PID_DiffBuf[i+1];
		
// 		if(TempOut==0)
// 		{
// 			PolarityZero++;
// 		}
// 		else if(TempOut>0)
// 		{
// 			PolarityPositive++;
// 		}
// 		else
// 		{
// 			PolarityNegative++;
// 		}
		
// 		DiffErr+= TempOut;
// 	}
	
// 	if(PolarityPositive>(PolarityNegative+PolarityZero))
// 	{
// 		pPid->Diff_LastPolarity = 1;
// 	}
// 	else if(PolarityNegative>(PolarityPositive+PolarityZero))
// 	{
// 		pPid->Diff_LastPolarity = -1;
// 	}
// 	else
// 	{
// 		pPid->Diff_LastPolarity = 0;
// 	}
	
	
// 	TempOut = pPid->Process_Kp * pPid->err + 
// 			  pPid->Process_Ki * pPid->Integral_err * Integral_Clutch / 200 +
// 			  pPid->Process_Kd * DiffErr;
	
// 	TempOut/=100;
// 	if(TempOut<0)
// 		TempOut = 0;
	
// 	pPid->ActualPosition = 	TempOut; 
	
// 	return pPid->ActualPosition;
// }
// int32_t temp_value;
// int32_t PIDIsA_Realize(PID *pPid,int32_t Current)
// {
// 	int32_t TempOut;
// 	int32_t Integral_Clutch;
// 	int32_t DiffErr;
	
// 	pPid->err = pPid->TargetPosition - Current;
	
// //	if(pPid->ActualPosition>=OUTPUT_SCALER_MAX)
// //	{
// //		if(pid_abs(pPid->Integral_err)>pPid->IntegralLimit)
// //		{
// //			Integral_Clutch = 0;
// //		}
// //		else
// //		{
// //			Integral_Clutch = 1;
// //			if(pPid->err<0)
// //			{
// //				//pPid->Integral_err += pPid->err* pPid->Process_Ki;
// //				pPid->Integral_err += pPid->err;
// //				
// //			}
// //		}
// //	}
// //	else if(pPid->ActualPosition<=-OUTPUT_SCALER_MAX)
// //	{
// //		if(pid_abs(pPid->Integral_err)>pPid->IntegralLimit)
// //		{
// //			Integral_Clutch = 0;
// //		}
// //		else
// //		{
// //			Integral_Clutch = 1;
// //			if(pPid->err>0)
// //			{
// //				//pPid->Integral_err += pPid->err* pPid->Process_Ki;
// //				pPid->Integral_err += pPid->err;
// //				
// //			}
// //		}
// //	}
// //	else
// 	{
// 		if(pPid->Integral_err > pPid->IntegralLimit)
// 		{
// 			if(pPid->err < 0)
// 			{
// 				pPid->Integral_err += pPid->err;
// 			}		
// 		}
// 		else
// 		{
// 			pPid->Integral_err += pPid->err;
// 		}
// 	}
	
// //	if(pPid->Integral_err<0)
// //		pPid->Integral_err = 0;
	
// 	DiffErr = 0;
// 	DiffErr = pPid->err - pPid->err_last;
// 	//DiffErr += pPid->err_last - pPid->err_Prev_1;
		
// 	TempOut = (pPid->Process_Kp * pPid->err + pPid->Process_Ki * pPid->Integral_err + pPid->Process_Kd * DiffErr) /1000;
	
// 	pPid->err_Prev_1 = pPid->err_last;
// 	pPid->err_last = pPid->err;
// 	temp_value = TempOut;


// 	pPid->ActualPosition = TempOut;
	
// 	if(pPid->ActualPosition > OUTPUT_SCALER_MAX)
// 	{
// 		pPid->ActualPosition = OUTPUT_SCALER_MAX;
// 	}
// 	else if(pPid->ActualPosition < -OUTPUT_SCALER_MAX)
// 	{
// 		pPid->ActualPosition = -OUTPUT_SCALER_MAX;
// 	}
	
// 	return pPid->ActualPosition;
// }
// /********************************(C)COPYRIGHT 2016 RSEStudio ****************************/
// /*****************************************END OF FILE************************************/

